#include "bspline_opt/bspline_optimizer.h"

namespace ego_planner {
void BsplineOptimizer::setParam(ros::NodeHandle &nh)
{
  nh.param("optimization/lambda_smooth_", lambda_smooth_, -1.0);
  nh.param("optimization/lambda_collision", lambda_collision_, -1.0);
  nh.param("optimization/lambda_mov_obj_", lambda_mov_obj_, -1.0);
  nh.param("optimization/lambda_feasibility", lambda_feasible_, -1.0);
  nh.param("optimization/lambda_fitness", lambda_fitness_, -1.0);
  nh.param("optimization/lambda_key_view_", lambda_key_view_, -1.0);

  nh.param("optimization/dist_safe", dist_safe_, -1.0);
  nh.param("optimization/dist_safe_dynamic", dist_safe_dyn_, -1.0);
  nh.param("optimization/dist_attention_", dist_attention_, -1.0);
  nh.param("optimization/swarm_clearance", swarm_clearance_, -1.0);
  nh.param("optimization/max_vel", max_vel_, -1.0);
  nh.param("optimization/max_acc", max_acc_, -1.0);
  nh.param("optimization/coff_slow_down_key_view_", coff_slow_down_key_view_, -1.0);

  max_vel_key_view_ = max_vel_ * coff_slow_down_key_view_;

  nh.param("optimization/order", order_, 3);
}

void BsplineOptimizer::setEnvironment(const EntropyMap::Ptr &map)
{
  grid_map_ = map;
  map_reso_ = grid_map_->getResolution();
}

void BsplineOptimizer::setPosControlPointsAndTs(Eigen::MatrixXd &ctrl_pts, double ts)
{
  setBsplineInterval(ts);
  cps_.resize(ctrl_pts.cols());
  cps_.points = ctrl_pts;
}

/**
 * @brief 通过将向量反向，来找到同一障碍物另一边的无障碍控制点，并将各段轨迹排列组合，生成最终轨迹
 * 一般segments.size 都是0/1/2 左右，不会太大
 *
 * @param  occ_segs_: the start and end ID of occ_segs_ in obstacles
 */
std::vector<ControlPoints> BsplineOptimizer::distinctiveTrajs()
{
  if (occ_segs_.size() == 0)
  {
    std::vector<ControlPoints> oneSeg;
    oneSeg.push_back(cps_);
    return oneSeg;
  }

  // 默认最多3段seg
  constexpr int MAX_TRAJS = 8;
  constexpr int VARIS = 2;
  int MAX_SEGS = static_cast<int>(floor(log(MAX_TRAJS) / log(VARIS)));
  int seg_upbound = std::min((int)occ_segs_.size(), MAX_SEGS);

  std::vector<ControlPoints> control_pts_buf;
  control_pts_buf.reserve(MAX_TRAJS);
  double CTRL_PT_DIST = (cps_.points.col(0) - cps_.points.rightCols(1)).norm() /
                        (cps_.points.cols() - 1);

  std::vector<std::pair<ControlPoints, ControlPoints>> RichInfoSegs;
  for (int i = 0; i < seg_upbound; i++)
  {
    std::pair<ControlPoints, ControlPoints> seg;
    ControlPoints seg_tmp;
    cps_.segment(seg_tmp, occ_segs_[i].first, occ_segs_[i].second);
    seg.first = seg_tmp;
    seg.second = seg_tmp;
    RichInfoSegs.push_back(seg);
  }

  /**
   * Step 1.
   * Find the opposite vectors and base points for every segment.
   */
  int i = 0;

step_1_start:;

  seg_upbound = std::min((int)occ_segs_.size(), MAX_SEGS);
  for (; i < seg_upbound; i++)
  {
    /* 1.1 Find the start occupied point id and the last occupied point id */
    if (RichInfoSegs[i].first.size_ > 1)
    {
      int occ_start_id = -1, occ_end_id = -1;
      Eigen::Vector3d occ_start_pt, occ_end_pt;
      for (int j = 0; j < RichInfoSegs[i].first.size_ - 1; j++)
      {
        Eigen::Vector3d start_pt = RichInfoSegs[i].first.points.col(j);
        Eigen::Vector3d end_pt = RichInfoSegs[i].first.points.col(j + 1);
        double step_size = map_reso_ / (start_pt - end_pt).norm() / 2;
        for (double a = 1; a > 0; a -= step_size)
        {
          Eigen::Vector3d pt(a * start_pt + (1 - a) * end_pt);
          if (grid_map_->getOccupancy(pt))
          {
            occ_start_id = j;
            occ_start_pt = pt;
            goto exit_multi_loop1;
          }
        }
      }
      // There are no collision points on this segment, early exit.
      goto exit_multi_loop2;

    exit_multi_loop1:;
      for (int j = RichInfoSegs[i].first.size_ - 1; j >= 1; j--)
      {
        Eigen::Vector3d start_pt = RichInfoSegs[i].first.points.col(j);
        Eigen::Vector3d end_pt = RichInfoSegs[i].first.points.col(j - 1);
        double step_size = map_reso_ / (start_pt - end_pt).norm();
        for (double a = 1; a > 0; a -= step_size)
        {
          Eigen::Vector3d pt(a * start_pt + (1 - a) * end_pt);
          if (grid_map_->getOccupancy(pt))
          {
            occ_end_id = j;
            occ_end_pt = pt;
            goto exit_multi_loop2;
          }
        }
      }

    exit_multi_loop2:;
      if (occ_start_id == -1 || occ_end_id == -1)
      {
        // It means the segment is free， abondaon
        occ_segs_.erase(occ_segs_.begin() + i);
        RichInfoSegs.erase(RichInfoSegs.begin() + i);
        goto step_1_start;
      }

      /* 1.2 Reverse the vector and find new base points from occ_start_id to occ_end_id. */
      for (int j = occ_start_id; j <= occ_end_id; j++)
      {
        Eigen::Vector3d occ_pt_reverse, dir_reverse;

        // TODO 不可能出现, 但是出现了 #3
        // TODO 其实是=0， 在initControlPoints里面没有处理好异常情况
        // TODO 现在还有 = 2的
        // RichInfoSegs[0].first.free_pts[0].size()= 6148910773712366000 没有初始化吗
        // RichInfoSegs[0].first.free_pts[0].size()=12297825457735299492
        if (RichInfoSegs[i].first.free_pts[j].size() != 1)
        {
          cout << "RichInfoSegs[" << i << "].first.free_pts[" << j << "].size()="
               << RichInfoSegs[i].first.free_pts[j].size() << endl;
          cout << " cps.size=" << cps_.size_ << endl;
          ROS_ERROR("Wrong number of base_points!!! Should not be happen!.");

          occ_segs_.erase(occ_segs_.begin() + i);
          RichInfoSegs.erase(RichInfoSegs.begin() + i);
          goto step_1_start;
        }

        // TODO 这里用最后一个方向会不会好一点
        dir_reverse = -RichInfoSegs[i].first.directions[j][0];

        // The start and the end case must get taken special care of.
        if (j == occ_start_id)
        {
          occ_pt_reverse = occ_start_pt;
        }
        else if (j == occ_end_id)
        {
          occ_pt_reverse = occ_end_pt;
        }
        else
        {
          occ_pt_reverse = RichInfoSegs[i].first.points.col(j) +
                           dir_reverse * (RichInfoSegs[i].first.free_pts[j][0] -
                                          RichInfoSegs[i].first.points.col(j))
                                             .norm();
        }

        // Search outward.
        if (grid_map_->getOccupancy(occ_pt_reverse))
        {
          double l_upbound = 5 * CTRL_PT_DIST; // "5" is the threshold.
          double l = map_reso_;
          for (; l <= l_upbound; l += map_reso_)
          {
            Eigen::Vector3d base_pt_temp = occ_pt_reverse + l * dir_reverse;
            if (!grid_map_->getOccupancy(base_pt_temp))
            {
              RichInfoSegs[i].second.free_pts[j][0] = base_pt_temp;
              RichInfoSegs[i].second.directions[j][0] = dir_reverse;
              break;
            }
          }
          if (l > l_upbound)
          {
            ROS_WARN("Can't find the new base points at the opposite within the threshold,Dont care");
            // TODO BUG  #5 这里之后死掉 i=0, j=0
            occ_segs_.erase(occ_segs_.begin() + i);
            RichInfoSegs.erase(RichInfoSegs.begin() + i);
            goto step_1_start;
          }
        }
        // occ_pt_reverse is free, unnecessary to search.
        else
        {
          RichInfoSegs[i].second.free_pts[j][0] = occ_pt_reverse;
          RichInfoSegs[i].second.directions[j][0] = dir_reverse;
        }
      }

      /* 1.3 Assign the base points to control points within [0, occ_start_id)
             and (occ_end_id, RichInfoSegs[i].first.size()-1]. */
      // TODO Why?
      if (RichInfoSegs[i].second.size_)
      {
        for (int j = occ_start_id - 1; j >= 0; j--)
        {
          RichInfoSegs[i].second.free_pts[j][0] = RichInfoSegs[i].second.free_pts[occ_start_id][0];
          RichInfoSegs[i].second.directions[j][0] = RichInfoSegs[i].second.directions[occ_start_id][0];
        }
        for (int j = occ_end_id + 1; j < RichInfoSegs[i].second.size_; j++)
        {
          RichInfoSegs[i].second.free_pts[j][0] = RichInfoSegs[i].second.free_pts[occ_end_id][0];
          RichInfoSegs[i].second.directions[j][0] = RichInfoSegs[i].second.directions[occ_end_id][0];
        }
      }
    }
    // just one point in seg
    else
    {
      Eigen::Vector3d dir_reverse = -RichInfoSegs[i].first.directions[0][0];
      Eigen::Vector3d occ_pt_reverse = RichInfoSegs[i].first.points.col(0) +
                                       dir_reverse * (RichInfoSegs[i].first.free_pts[0][0] -
                                                      RichInfoSegs[i].first.points.col(0))
                                                         .norm();

      if (grid_map_->getOccupancy(occ_pt_reverse)) // Search outward.
      {
        double l_upbound = 5 * CTRL_PT_DIST; // "5" is the threshold.
        double l = map_reso_;
        for (; l <= l_upbound; l += map_reso_)
        {
          Eigen::Vector3d base_pt_temp = occ_pt_reverse + l * dir_reverse;
          if (!grid_map_->getOccupancy(base_pt_temp))
          {
            RichInfoSegs[i].second.free_pts[0][0] = base_pt_temp;
            RichInfoSegs[i].second.directions[0][0] = dir_reverse;
            break;
          }
        }
        if (l > l_upbound)
        {
          ROS_WARN("Can't find the new base points at the opposite within the threshold, 2. i=%d", i);
          occ_segs_.erase(occ_segs_.begin() + i);
          RichInfoSegs.erase(RichInfoSegs.begin() + i);
          goto step_1_start;
        }
      }
      else {
        RichInfoSegs[i].second.free_pts[0][0] = occ_pt_reverse;
        RichInfoSegs[i].second.directions[0][0] = dir_reverse;
      }
    }
  }

  /**
   * Step 2
   *  Assemble each segment to make up the new control point sequence.
   */

  // After the erase operation above, segment legth will decrease to 0 again.
  if (seg_upbound == 0)
  {
    std::vector<ControlPoints> oneSeg;
    oneSeg.push_back(cps_);
    return oneSeg;
  }

  // 对应着[0,0],[1,0],[0,1]..2^n种情况，用以组合不同部分的轨迹
  std::vector<int> selection(seg_upbound, 0);
  selection[0] = -1; // init
  int traj_nums = static_cast<int>(pow(VARIS, seg_upbound));
  for (int i = 0; i < traj_nums; i++)
  {
    // 2.1 Calculate the selection table.
    int index = 0;
    selection[index]++;
    while (index < seg_upbound && selection[index] >= VARIS)
    {
      selection[index] = 0;
      index++;
      if (index >= seg_upbound)
      {
        ROS_ERROR("Should not happen!!! index=%d, seg_upbound=%d", index, seg_upbound);
      }
      selection[index]++;
    }

    // 2.2 Assign params according to the selection table.
    ControlPoints cpsOneSample;
    cpsOneSample.resize(cps_.size_);
    int cp_id = 0, seg_id = 0, cp_of_seg_id = 0;
    while (cp_id < cps_.size_)
    {
      // 对于非occ的点
      if (seg_id >= seg_upbound || cp_id < occ_segs_[seg_id].first || cp_id > occ_segs_[seg_id].second)
      {
        cpsOneSample.points.col(cp_id) = cps_.points.col(cp_id);
        cpsOneSample.free_pts[cp_id] = cps_.free_pts[cp_id];
        cpsOneSample.directions[cp_id] = cps_.directions[cp_id];
      }
      else if (cp_id >= occ_segs_[seg_id].first && cp_id <= occ_segs_[seg_id].second)
      {
        if (!selection[seg_id])
        {
          cpsOneSample.points.col(cp_id) = RichInfoSegs[seg_id].first.points.col(cp_of_seg_id);
          cpsOneSample.free_pts[cp_id] = RichInfoSegs[seg_id].first.free_pts[cp_of_seg_id];
          cpsOneSample.directions[cp_id] = RichInfoSegs[seg_id].first.directions[cp_of_seg_id];
          cp_of_seg_id++;
        }
        else
        {

          cpsOneSample.points.col(cp_id) = RichInfoSegs[seg_id].second.points.col(cp_of_seg_id);
          cpsOneSample.free_pts[cp_id] = RichInfoSegs[seg_id].second.free_pts[cp_of_seg_id];
          cpsOneSample.directions[cp_id] = RichInfoSegs[seg_id].second.directions[cp_of_seg_id];
          cp_of_seg_id++;
        }

        if (cp_id == occ_segs_[seg_id].second)
        {
          cp_of_seg_id = 0;
          seg_id++;
        }
      }
      else
      {
        ROS_ERROR("Shold not happen!!!!, cp_id=%d, seg_id=%d, occ_segs_.front().first=%d",
                  cp_id, seg_id, occ_segs_.front().first);
        ROS_ERROR("occ_segs_.back().second=%d, occ_segs_[seg_id].first=%d, occ_segs_[seg_id].second=%d",
                  occ_segs_.back().second, occ_segs_[seg_id].first, occ_segs_[seg_id].second);
      }

      cp_id++;
    }

    control_pts_buf.push_back(cpsOneSample);
  }

  return control_pts_buf;
}

/**
 * @brief Init the control points.
 *        Then check collision of the initial trajectory
 *        and generate the direction and first free point of
 *        each control point that is occ in [checkCollisionFine].
 *
 * @return 没用
 */
bool BsplineOptimizer::InitAndProcess(const Eigen::MatrixXd &pts_init)
{
  // Inilialize the control points.
  cps_.resize(pts_init.cols());
  cps_.points = pts_init;

  return checkCollisionFine();
}

/**
 * @brief check collision in a FINE way,
 *        if collision happens, drag the segment out of obstacles.
 *
 * @note Used in the initialization process
 *       or one optimization iteration completed
 *
 * @return 没用
 */
bool BsplineOptimizer::checkCollisionFine()
{
  /*** Step1 Segment the initial trajectory according to obstacles ***/
  constexpr int ENOUGH_INTERVAL = 2;
  int same_state_times = ENOUGH_INTERVAL + 1;
  double step_size = map_reso_ / 1.5 /
                     ((cps_.points.col(0) - cps_.points.rightCols(1)).norm() /
                      (cps_.points.cols() - 1));
  int in_id = -1, out_id = -1;
  std::vector<std::pair<int, int>> segment_ids;
  bool occ = false;
  bool last_occ = false;
  bool flag_got_start = false, flag_got_end = false, flag_got_end_maybe = false;

  // only check closed 2/3 points
  // 减去order_的意义: 他用的是均匀bspline，前后有几个点是不能用的
  // TODO 这个order感觉最好是在bspline里面指定
  int i_end = (int)cps_.points.cols() - order_ -
              ((int)cps_.points.cols() - 2 * order_) / 3;
  for (int i = order_; i <= i_end; ++i)
  {
    // after test (start_pt - end_pt).norm() = 0.4
    Eigen::Vector3d start_pt(cps_.points.col(i - 1));
    Eigen::Vector3d end_pt(cps_.points.col(i));

    // after test 0.16 < step_size < 0.2
    for (double a = 1.0; a > 0.0; a -= step_size)
    {
      occ = grid_map_->getOccupancy(a * start_pt + (1 - a) * end_pt);

      if (occ && !last_occ)
      {
        if (same_state_times > ENOUGH_INTERVAL)
        {
          in_id = i - 1;
          flag_got_start = true;
        }
        flag_got_end_maybe = false;
        same_state_times = 0;
      }
      else if (!occ && last_occ)
      {
        out_id = i;
        flag_got_end_maybe = true;
        same_state_times = 0;
      }
      else
      {
        ++same_state_times;
      }

      if (flag_got_end_maybe && (same_state_times > ENOUGH_INTERVAL ||
                                 (i == i_end && a <= step_size)))
      {
        flag_got_end_maybe = false;
        flag_got_end = true;
      }

      last_occ = occ;

      if (flag_got_start && flag_got_end)
      {
        flag_got_start = false;
        flag_got_end = false;

        // avoid overlap
        if (!segment_ids.empty() && segment_ids.back().first == in_id)
          segment_ids.back().second = out_id;
        else
          segment_ids.push_back(std::pair<int, int>(in_id, out_id));
      }
    }
  }

  /*** Step2  ***/
  return dragSegOutofObs(segment_ids, true);
}

/**
 * @brief Check collision in a COARSE way,
 *        if collision happens, drag the segment out of obstacles.
 *
 * @note Used in the optimization process
 *
 * @return return的Value没用
 */
bool BsplineOptimizer::checkCollisionCoarse()
{
  /*** Step1 Segment the initial trajectory according to obstacles ***/
  int in_id, out_id;
  vector<std::pair<int, int>> segment_ids;
  bool flag_new_obs_valid = false;
  int end_idx = cps_.size_ - order_;
  int i_end = end_idx - (end_idx - order_) / 3;
  for (int i = order_ - 1; i <= i_end; ++i)
  {
    bool occ = grid_map_->getOccupancy(cps_.points.col(i));

    if (occ)
    {
      flag_new_obs_valid = true;

      int j;
      for (j = i - 1; j >= 0; --j)
      {
        occ = grid_map_->getOccupancy(cps_.points.col(j));
        if (!occ)
        {
          in_id = j;
          break;
        }
      }
      if (j < 0) // fail to get the obs free point
      {
        ROS_ERROR("ERROR! the drone is in obstacle. This should not happen. BUG.");
        in_id = 0;
      }

      for (j = i + 1; j < cps_.size_; ++j)
      {
        occ = grid_map_->getOccupancy(cps_.points.col(j));

        if (!occ)
        {
          out_id = j;
          break;
        }
      }
      if (j >= cps_.size_) // fail to get the obs free point
      {
        // TODO bug #10
        // 其实终点在里面无所谓的，扔给Astar也能解决
        // 如果下次发生了，可以直接把终点扔给Astar
        ROS_WARN("WARN! terminal point of the current trajectory is in obstacle, skip this planning.");
        // force_stop_type_ = STOP_FOR_ERROR;
        // return false;
        out_id = cps_.size_ - 1;
      }

      i = j + 1;

      segment_ids.push_back(std::pair<int, int>(in_id, out_id));
    }
  }

  if (!flag_new_obs_valid)
    return true;

  dragSegOutofObs(segment_ids, false);

  force_stop_type_ = STOP_FOR_REBOUND;
  return true;
}

/**
 * @brief Drag the segment out of obstacles,obtaining directions and free points,
 *        stored in [cps_.free_pts] and [cps_.directions]
 *
 * @note: the core of EGO-Planner
 *
 * @param segment_ids: the start and end ID of occ_segs_ in obstacles
 * @param recordSeg: whether to record the segment for [distinctiveTrajs]
 *
 * @return 没用的
 */
bool BsplineOptimizer::dragSegOutofObs(
    std::vector<std::pair<int, int>> segment_ids,
    bool recordSeg)
{
  occ_segs_.clear();
  size_t seg_size = segment_ids.size();

  if (seg_size == 0) {
    return false;
  }

  /* Avoid overlap */
  // TODO 先加这，但我感觉不会发生，可以回头看看上面segment_ids分配时候的代码
  for (size_t i = 1; i < seg_size; i++)
  {
    // 两段距离很近时，直接合并
    if (segment_ids[i - 1].second == segment_ids[i].first ||
        segment_ids[i - 1].second + 1 == segment_ids[i].first)
    {
      segment_ids[i - 1].second = segment_ids[i].second;
      segment_ids.erase(segment_ids.begin() + i);
      i--;
      seg_size--;
    }
    else if (segment_ids[i - 1].second > segment_ids[i].first) {
      cout << "\033[1;31m[Bspline Opt]: segment_ids overlap! \033[0m" << endl;
      cout << "\033[1;31m[Bspline Opt]: segment_ids overlap! \033[0m" << endl;
      cout << "\033[1;31m[Bspline Opt]: segment_ids overlap! \033[0m" << endl;
      cout << "[i-1]" << segment_ids[i - 1].first << "->" << segment_ids[i - 1].second << endl;
      cout << "[i]" << segment_ids[i].first << "->" << segment_ids[i].second << endl;
      cout << "[i-1].second is occ: "
           << grid_map_->getGridState(cps_.points.col(segment_ids[i - 1].second)) << endl;
      cout << "[i].first is occ: "
           << grid_map_->getGridState(cps_.points.col(segment_ids[i].first)) << endl;
      double middle = (double)(segment_ids[i - 1].second + segment_ids[i].first) / 2.0;
      segment_ids[i - 1].second = static_cast<int>(middle - 0.1);
      segment_ids[i].first = static_cast<int>(middle + 1.1);
    }
  }

  /*** a star search ***/
  vector<vector<Eigen::Vector3d>> Astar_pathes;
  for (size_t i = 0; i < seg_size; ++i)
  {
    Eigen::Vector3d in(cps_.points.col(segment_ids[i].first));
    Eigen::Vector3d out(cps_.points.col(segment_ids[i].second));
    int ret = Astar_->AstarSearch(in, out);

    if (ret == AStar::SUCCESS) {
      Astar_pathes.push_back(Astar_->getPath());
      continue;
    }
    else if (ret == AStar::ERR_INVALIDINPUT || ret == AStar::ERR_ONLYONEPOINT) {
      cerr << "\033[1;31m[Astar]: " << AStar::getError(ret) << " \033[0m" << endl;
      saveAstarToTxt(Astar_->getPath(), "dragSegOutofObs.txt");
      saveConotrolPointsToTxt(
          cps_.points.block(0, segment_ids[i].first,
                            cps_.points.rows(),
                            segment_ids[i].second - segment_ids[i].first + 1),
          "dragSegOutofObs.txt");
      segment_ids.erase(segment_ids.begin() + i);
      i--;
      seg_size--;
      continue;
    }

    /* Deal with the case when astar is ERRROR */
    /* Step 1 find out the point which is real free not unknow */
    int index = segment_ids[i].second;
    int index_end;
    if (i < seg_size - 1) {
      index_end = segment_ids[i + 1].first;
    }
    else {
      index_end = (int)cps_.points.cols() - 1;
    }

    bool flag_in = false, flag_out = false;
    while (index < index_end) {
      Eigen::Vector3d now(cps_.points.col(index));
      Eigen::Vector3d next(cps_.points.col(index + 1));
      double step = map_reso_ / (now - next).norm();
      for (double a = 1.0 - step; a >= 0.0; a -= step) {
        bool occ = grid_map_->getOccupancy(a * now + (1 - a) * next);
        if (occ) {
          flag_in = true;
          continue;
        }
        if (flag_in && !occ) {
          flag_out = true;
          break;
        }
      }
      if (flag_out) break;
      index++;
    }

    /* Step 2 according to the flag_in and flag_out to deal with the segment */
    if (!flag_in) {
      /* Case 1: dont find any occ pt in the segment, delete it, is a BUG */
      cerr << YELLOW << " [Bspline Opt]: end pt is free, this impossible, Dontcare!"
           << RESET << endl;
      segment_ids.erase(segment_ids.begin() + i);
      i--;
      seg_size--;
    }
    else {
      /* Case 2: the first occ pt is found */
      if (!flag_out) {
        /* Case 2.1 : until the last pt is not free */
        cerr << YELLOW << "[Bspline Opt]: until the last pt is not free!"
             << RESET << endl;
        if (i < seg_size - 1) {
          segment_ids[i].second = segment_ids[i + 1].second;
          segment_ids.erase(segment_ids.begin() + i + 1);
          seg_size--;
        }
        else {
          segment_ids[i].second = index_end;
        }
      }
      else {
        /* Case 2.2 : find a free pt after occ pts */
        segment_ids[i].second = index + 1;
      }

      /* Do Astar search again */
      out = cps_.points.col(segment_ids[i].second);
      ret = Astar_->AstarSearch(in, out);
      if (ret == AStar::SUCCESS) {
        Astar_pathes.push_back(Astar_->getPath());
      }
      else {
        cerr << YELLOW << "[Astar]: " << AStar::getError(ret) << endl
             << "[Bspline Opt]: Twice star error, delete!" << RESET << endl;
        segment_ids.erase(segment_ids.begin() + i);
        i--;
        seg_size--;
      }
    }
  }

  if (seg_size == 0) {
    cerr << YELLOW << "[Bspline Opt]: a star error, force return!"
         << RESET << endl;
    return false;
  }

  /*** Assign free_pt and direction to each segment ***/
  for (size_t i = 0; i < seg_size; i++)
  {
    int begin_id = segment_ids[i].first;
    int end_id = segment_ids[i].second;
    std::vector<Eigen::Vector3d> Astar_path = Astar_pathes[i];

    /*** step 1 ***/
    // TODO 这个的范围再斟酌一下
    for (int j = begin_id; j <= end_id; ++j)
      cps_.flag_temp[j] = false;

    /*** step 2 ***/
    int got_intersection_id = -1;
    for (int j = begin_id + 1; j < end_id; ++j)
    {
      /**
       * step 2.1
       *  get intersect point on Astar path,
       *  which is in the normal plane of control point
       */
      // v_tangent: Ri = (Q_{i+1}-Q_{i-1}) , the Tangent line at point Qi
      Eigen::Vector3d v_tangent(cps_.points.col(j + 1) - cps_.points.col(j - 1));
      Eigen::Vector3d intersection_pt;
      Eigen::Vector3d cpt = cps_.points.col(j);

      // Let "Astar_id = id_of_the_most_far_away_Astar_point" will be better,
      // but it needs more computation
      size_t size_astar = Astar_path.size(); // after test 18-22
      size_t Astar_id = size_astar / 2;
      size_t last_Astar_id;
      // val: the cos value
      double val = (Astar_path[Astar_id] - cpt).dot(v_tangent);
      double last_val;
      while (Astar_id >= 0 && Astar_id < size_astar)
      {
        last_Astar_id = Astar_id;

        if (val >= 0)
          --Astar_id;
        else
          ++Astar_id;

        last_val = val;
        val = (Astar_path[Astar_id] - cpt).dot(v_tangent);

        // val = last_val = 0.0 is not allowed
        if (val * last_val <= 0 && (abs(val) > 0 || abs(last_val) > 0))
        {
          // 在fig3.b上，用三角投影能解出来, 线性插值
          intersection_pt =
              Astar_path[Astar_id] +
              ((Astar_path[Astar_id] - Astar_path[last_Astar_id]) *
               (v_tangent.dot(cpt - Astar_path[Astar_id]) /
                v_tangent.dot(Astar_path[Astar_id] - Astar_path[last_Astar_id])));
          got_intersection_id = j;
          break;
        }
      }

      /**
       * step 2.2
       *  get the [direction] of control point to intersect point
       *  get the first [free point] on the [direction]
       */
      if (got_intersection_id >= 0)
      {
        double length = (intersection_pt - cpt).norm();
        if (length > 0.01)
        {
          for (double a = length; a >= 0.0; a -= map_reso_)
          {
            Eigen::Vector3d pt = (a / length) * intersection_pt +
                                 (1 - a / length) * cpt;
            bool occ = grid_map_->getOccupancy(pt);
            if (occ || a < map_reso_) {
              if (occ) {
                a += map_reso_;
                pt = (a / length) * intersection_pt +
                     (1 - a / length) * cpt;

                if (a > length) {
                  if (grid_map_->getOccupancy(pt)) {
                    // TODO BUG #4
                    ROS_ERROR("[BSplibe Opt]: a>length, still occ, BUG!");
                  }
                }
              }
              cps_.flag_temp[j] = true;
              cps_.free_pts[j].push_back(pt);
              cps_.directions[j].push_back((intersection_pt - cpt).normalized());
              break;
            }
          }
        }
        else
        {
          // TODO 感觉这个大概率不会发生 如果发生了，可以改为直接用Astar_path[Astar_id]作为free_pt
          // 其实就是我在notion里面记录的情况，经常发生
          cout << "\033[1;31m[Bspline Opt]: TODO: intersection_pt is close to cps\033[0m" << endl;
          cout << "\033[1;31m[Bspline Opt]: TODO: intersection_pt is close to cps\033[0m" << endl;
          cout << "cpt" << cpt.transpose() << " state is " << grid_map_->getGridState(cpt) << endl;
          cout << "intersection_pt" << intersection_pt.transpose() << " state is "
               << grid_map_->getGridState(intersection_pt) << endl;

          got_intersection_id = -1;
        }
      }
      else
      {
        // Just for a point in a seg dont find intersect pt, it does not matter ^_^.
        // if all the point in a seg dont find intersect pt, it will be a problem.
      }
    }

    /**
     * Step2 Corner case:
     * the segment length is too short. Here the control points may outside the A* path,
     * leading to opposite gradient directions. So I have to take special care of it
     */
    if (end_id - begin_id == 1)
    {
      Eigen::Vector3d begin_pt(cps_.points.col(begin_id));
      Eigen::Vector3d end_pt(cps_.points.col(end_id));
      Eigen::Vector3d middle_pt = (end_pt + begin_pt) / 2;
      Eigen::Vector3d v_tangent(end_pt - begin_pt);
      Eigen::Vector3d intersection_pt;
      int Astar_id = Astar_path.size() / 2;
      int last_Astar_id;
      double val = (Astar_path[Astar_id] - middle_pt).dot(v_tangent);
      double last_val;
      while (Astar_id >= 0 && Astar_id < (int)Astar_path.size())
      {
        last_Astar_id = Astar_id;

        if (val >= 0)
          --Astar_id;
        else
          ++Astar_id;

        last_val = val;
        val = (Astar_path[Astar_id] - middle_pt).dot(v_tangent);

        if (val * last_val <= 0 && (abs(val) > 0 || abs(last_val) > 0))
        {
          intersection_pt =
              Astar_path[Astar_id] +
              ((Astar_path[Astar_id] - Astar_path[last_Astar_id]) *
               (v_tangent.dot(middle_pt - Astar_path[Astar_id]) /
                v_tangent.dot(Astar_path[Astar_id] - Astar_path[last_Astar_id])));

          if ((intersection_pt - middle_pt).norm() > 0.01) // 1cm.
          {
            cps_.flag_temp[begin_id] = true;
            cps_.free_pts[begin_id].push_back(begin_pt);
            cps_.directions[begin_id].push_back(
                (intersection_pt - middle_pt).normalized());

            got_intersection_id = begin_id;
          }
          break;
        }
      }
    }

    /**
     * Step 3
     *  Assign the free points and directions to the rest control points
     *  in the segment
     */
    if (got_intersection_id >= 0)
    {
      for (int j = got_intersection_id + 1; j <= end_id; ++j)
        if (!cps_.flag_temp[j])
        {
          // cout << "\033[1;31m dont " << j << ": "
          //      << cps_.points.col(j).transpose() << " \033[0m" << endl;
          // cout << "\033[1;31m begin_id " << begin_id << "end_id: "
          //      << end_id << " \033[0m" << endl;
          cps_.free_pts[j].push_back(cps_.free_pts[j - 1].back());
          cps_.directions[j].push_back(cps_.directions[j - 1].back());
        }

      for (int j = got_intersection_id - 1; j >= begin_id; --j)
        if (!cps_.flag_temp[j])
        {
          // cout << "\033[1;31m dont " << j << ": "
          //      << cps_.points.col(j).transpose() << " \033[0m" << endl;
          // cout << "\033[1;31m begin_id " << begin_id << "end_id: "
          //      << end_id << " \033[0m" << endl;
          cps_.free_pts[j].push_back(cps_.free_pts[j + 1].back());
          cps_.directions[j].push_back(cps_.directions[j + 1].back());
        }

      if (recordSeg)
        occ_segs_.push_back(segment_ids[i]);
    }
    else
    {
      // Just ignore, it does not matter ^_^.
      cout << "\033[1;31m seg_size " << seg_size << " \033[0m" << endl;
      cout << "\033[1;31m begin_id " << begin_id << "end_id: "
           << end_id << " \033[0m" << endl;
      cout << "\033[1;31m Failed to generate directions! segment_id "
           << i << " \033[0m" << endl;
      saveAstarToTxt(Astar_path, "dragSegOutofObs.txt");
      saveConotrolPointsToTxt(
          cps_.points.block(0, begin_id, cps_.points.rows(), end_id - begin_id + 1),
          "dragSegOutofObs.txt");
    }
  }

  return true;
}

int BsplineOptimizer::earlyExit(void *instance,
                                const Eigen::VectorXd &x,
                                const Eigen::VectorXd &g,
                                const double fx,
                                const double step,
                                const int k,
                                const int ls)
{
  BsplineOptimizer *opt = reinterpret_cast<BsplineOptimizer *>(instance);
  return (opt->force_stop_type_ == STOP_FOR_ERROR ||
          opt->force_stop_type_ == STOP_FOR_REBOUND);
}

double BsplineOptimizer::costFunction(void *instance,
                                      const Eigen::VectorXd &x,
                                      Eigen::VectorXd &grad)
{
  BsplineOptimizer *opt = reinterpret_cast<BsplineOptimizer *>(instance);

  double cost;
  opt->combineCost(x, grad, cost);

  opt->iter_num_ += 1;
  return cost;
}

double BsplineOptimizer::costFunctionRefine(void *instance,
                                            const Eigen::VectorXd &x,
                                            Eigen::VectorXd &g)
{
  BsplineOptimizer *opt = reinterpret_cast<BsplineOptimizer *>(instance);

  double cost;
  opt->combineCostRefine(x, g, cost);
  opt->iter_num_ += 1;
  return cost;
}

bool BsplineOptimizer::OptimizeTraj(Eigen::MatrixXd &optimal_points, double ts)
{
  setBsplineInterval(ts);

  double final_cost;
  bool flag_success = optimize(final_cost);

  optimal_points = cps_.points;

  return flag_success;
}

bool BsplineOptimizer::OptimizeTraj(Eigen::MatrixXd &optimal_points, double &final_cost,
                                    const ControlPoints &control_points, double ts)
{
  setBsplineInterval(ts);

  cps_ = control_points;

  bool flag_success = optimize(final_cost);

  optimal_points = cps_.points;

  return flag_success;
}

bool BsplineOptimizer::OptimizeTrajRefine(const Eigen::MatrixXd &pts_init, const double ts, Eigen::MatrixXd &optimal_points)
{

  setControlPoints(pts_init);
  setBsplineInterval(ts);

  bool flag_success = optimize_refine();

  optimal_points = cps_.points;

  return flag_success;
}

/**
 * construct a optimal problem and  optimize Control Points
 */
bool BsplineOptimizer::optimize(double &final_cost)
{
  iter_num_ = 0;
  int start_id = order_; // 前order_个控制点对起始的状态有影响，所以不能动?
  // int end_id = cps_.size_ - order_; // Fixed end
  int end_id = cps_.size_; // Free end
  variable_num_ = 3 * (end_id - start_id);

  // prepare the dynamic obstacles position
  dyn_obs_cp_index_.clear();
  dyn_obs_cp_index_.resize(obs_trajs_->size());
  dyn_obs_pos_.clear();
  dyn_obs_pos_.resize(obs_trajs_->size());
  std::vector<int> check_ids;

  ros::Time t_ros_now = ros::Time::now();

  // determine the dynamic obstacles position
  for (size_t i = 0; i < obs_trajs_->size(); i++)
  {
    check_ids.push_back(obs_trajs_->at(i)->id);
    UniformBspline obs_traj = obs_trajs_->at(i)->traj_pos_;
    double t_now_obs = (t_ros_now - obs_trajs_->at(i)->start_time_).toSec();
    double dura_obs = obs_traj.getTimeSum();

    for (int j = start_id; j < cps_.size_ - order_; j++)
    {
      double t = t_now_obs +
                 UniformBspline::getControlPtMaxT(j, order_, bspline_interval_);
      if (t < dura_obs)
      {
        dyn_obs_pos_[i].push_back(obs_traj.getPosT(t));
        dyn_obs_cp_index_[i].push_back(j);
      }
      else
        break;
    }
  }

  // for dynamic obstacles just appear
  for (size_t i = 0; i < obs_poses_->size(); i++) {
    int id = obs_poses_->at(i)->id;
    if (std::find(check_ids.begin(), check_ids.end(), id) != check_ids.end()) {
      continue;
    }
    if (obs_poses_->at(i)->time + ros::Duration(0.5) < t_ros_now) {
      continue;
    }
    Eigen::Vector3d pos = obs_poses_->at(i)->pos;
    std::vector<Eigen::Vector3d> pos_vec;
    std::vector<int> index_vec;
    for (int j = start_id; j < cps_.size_ - order_ && j < start_id + 3; j++)
    {
      pos_vec.push_back(pos);
      index_vec.push_back(j);
    }
    dyn_obs_pos_.push_back(pos_vec);
    dyn_obs_cp_index_.push_back(index_vec);
    cout << GREEN_B << "[Bspline Opt]: Dynamic obs appear, id=" << id << RESET << endl;
  }

  ros::Time t0 = ros::Time::now(), t1, t2;

  int reoptmize_times = 0; // the opt result is collision, re-optimize
  int restart_times = 0;   // the opt process is canceled, restart opt
  constexpr int MAX_REOPTMIZE_TIMES = 3;
  constexpr int MAX_RESTART_TIMES = 20;
  bool flag_restart, flag_occ, success;
  new_lambda_collision_ = lambda_collision_;

  lbfgs::lbfgs_parameter_t lbfgs_params;
  lbfgs_params.mem_size = 16;
  lbfgs_params.max_iterations = 200;
  lbfgs_params.g_epsilon = 0.01;

  Eigen::VectorXd vec(variable_num_);
  do
  {
    /* ---------- prepare ---------- */
    iter_num_ = 0;
    flag_restart = false;
    flag_occ = false;
    success = false;

    memcpy(vec.data(), cps_.points.data() + 3 * start_id,
           variable_num_ * sizeof(double));

    /* ---------- optimize ---------- */
    t1 = ros::Time::now();
    int result = lbfgs::lbfgs_optimize(vec,
                                       final_cost,
                                       BsplineOptimizer::costFunction,
                                       nullptr,
                                       BsplineOptimizer::earlyExit,
                                       this,
                                       lbfgs_params);

    t2 = ros::Time::now();
    double time_ms = (t2 - t1).toSec() * 1000;
    double total_time_ms = (t2 - t0).toSec() * 1000;

    /* ---------- success temporary, check collision again ---------- */
    if (result == lbfgs::LBFGS_CONVERGENCE ||
        result == lbfgs::LBFGSERR_MAXIMUMITERATION ||
        result == lbfgs::LBFGS_STOP)
    {
      /*** collision check, phase 2 ***/
      UniformBspline traj = UniformBspline(cps_.points, order_, bspline_interval_);
      double tm, tmp;
      traj.getTimeSpan(tm, tmp);
      double t_step = (tmp - tm) / ((traj.getPosT(tmp) - traj.getPosT(tm)).norm() / map_reso_);
      // Only check the closest 2/3 partition of the whole trajectory.
      double t_end = (tmp - tm) * 2 / 3 + tm;
      for (double t = tm; t < t_end; t += t_step)
      {
        flag_occ = grid_map_->getOccupancy(traj.getPosT(t));
        if (flag_occ)
        {
          if (t <= bspline_interval_) // First 2 control points in obstacles!
          {
            printf("\033[1;31m[Bspline Opt]: First 2 points in occ! return false, t=%f\n\033[0m", t);
            return false;
          }
          break;
        }
      }
      if (!flag_occ)
      {
        // cout.precision(4);
        // cout << "\033[32m[OPT]:iter=" << iter_num_ << ", reStart=" << restart_times
        //      << ", reOpt=" << reoptmize_times << ", time(ms)=" << time_ms
        //      << ", total_t(ms)=" << total_time_ms << ", cost=" << final_cost << "\033[0m" << endl;
        success = true;
      }
      else
      { /* Collision, continue opt by increasing collision weight */
        reoptmize_times++;
        checkCollisionFine();
        new_lambda_collision_ *= 2;

        // printf("\033[32m[OPT]: iter = %d,time(ms) = %5.3f, collided, keep optimizing\n\033[0m",
        //        iter_num_, time_ms);
      }
    }
    else if (result == lbfgs::LBFGSERR_CANCELED)
    {
      flag_restart = true;
      restart_times++;
      // printf("\033[32m[OPT]: iter = %d,time(ms) = %5.3f, restart \n\033[0m", iter_num_, time_ms);
    }
    else
    {
      printf("\033[1;33m[Bspline Opt]: Solver error. Return = %d, %s. Skip this planning \n\033[0m",
             result, lbfgs::lbfgs_strerror(result));
    }
  } while ((flag_occ && reoptmize_times < MAX_REOPTMIZE_TIMES) ||
           (flag_restart && force_stop_type_ == STOP_FOR_REBOUND &&
            restart_times <= MAX_RESTART_TIMES));

  return success;
}

bool BsplineOptimizer::optimize_refine()
{
  iter_num_ = 0;
  int start_id = order_;
  int end_id = cps_.points.cols() - order_;
  variable_num_ = 3 * (end_id - start_id);

  double final_cost;

  Eigen::VectorXd vec(variable_num_);
  memcpy(vec.data(), cps_.points.data() + 3 * start_id, variable_num_ * sizeof(double));

  lbfgs::lbfgs_parameter_t lbfgs_params;
  lbfgs_params.mem_size = 16;
  lbfgs_params.max_iterations = 200;
  lbfgs_params.g_epsilon = 0.001;

  double origin_lambda_fitness = lambda_fitness_;
  bool flag_safe = true;
  int iter_count = 0;
  do
  {
    int result = lbfgs::lbfgs_optimize(vec,
                                       final_cost,
                                       BsplineOptimizer::costFunctionRefine,
                                       nullptr,
                                       nullptr,
                                       this,
                                       lbfgs_params);

    if (result == lbfgs::LBFGS_CONVERGENCE ||
        result == lbfgs::LBFGSERR_MAXIMUMITERATION ||
        result == lbfgs::LBFGS_STOP)
    {
      // pass
    }
    else
    {
      ROS_ERROR("Solver error in refining!, return = %d, %s", result, lbfgs::lbfgs_strerror(result));
    }

    UniformBspline traj = UniformBspline(cps_.points, 3, bspline_interval_);
    double tm = 0, tmp = 0;
    traj.getTimeSpan(tm, tmp);
    // Step size is defined as the maximum size that can passes throgth every gird.
    double t_step = (tmp - tm) / ((traj.getPosT(tmp) - traj.getPosT(tm)).norm() / map_reso_);
    double t_end = (tmp - tm) * 2 / 3 + tm;
    for (double t = tm; t < t_end; t += t_step)
    {
      if (grid_map_->getOccupancy(traj.getPosT(t)))
      {
        flag_safe = false;
        break;
      }
    }

    if (!flag_safe)
      lambda_fitness_ *= 2;

    iter_count++;
  } while (!flag_safe && iter_count <= 0);

  lambda_fitness_ = origin_lambda_fitness;

  return flag_safe;
}

void BsplineOptimizer::combineCost(const Eigen::VectorXd &x,
                                   Eigen::VectorXd &grad,
                                   double &f)
{
  // 给cps_.points赋值
  size_t n = x.size();
  memcpy(cps_.points.data() + 3 * order_, x.data(), n * sizeof(double));

  /* ---------- evaluate cost and gradient ---------- */
  double f_smoothness = 0.0,
         f_distance = 0.0,
         f_feasibility = 0.0,
         f_mov_objs = 0.0,
         f_key_view = 0.0;

  Eigen::MatrixXd g_smoothness = Eigen::MatrixXd::Zero(3, cps_.size_);
  Eigen::MatrixXd g_distance = Eigen::MatrixXd::Zero(3, cps_.size_);
  Eigen::MatrixXd g_feasibility = Eigen::MatrixXd::Zero(3, cps_.size_);
  Eigen::MatrixXd g_mov_objs = Eigen::MatrixXd::Zero(3, cps_.size_);
  Eigen::MatrixXd g_key_view = Eigen::MatrixXd::Zero(3, cps_.size_);
  // Eigen::MatrixXd g_swarm_attract = Eigen::MatrixXd::Zero(3, cps_.size_);
  // Eigen::MatrixXd g_swarm = Eigen::MatrixXd::Zero(3, cps_.size_);
  // Eigen::MatrixXd g_terminal = Eigen::MatrixXd::Zero(3, cps_.size_);

  calcSmoothnessCost(cps_.points, f_smoothness, g_smoothness);
  // EGO-PLANNER
  calcDistanceCostRebound(
      cps_.points, f_distance, g_distance, iter_num_, f_smoothness);
  // FAST-PLANNER
  // calcESDFReboundCost(cps_.points, f_distance, g_distance);
  calcFeasibilityCost(cps_.points, f_feasibility, g_feasibility);
  calcMovingObjCost(cps_.points, f_mov_objs, g_mov_objs);
  // calcKeyViewPtCost(cps_.points, f_key_view, g_key_view);
  // calcTerminalCost(cps_.points, f_terminal, g_terminal);

  // cout << GREEN_B
  //      << f_smoothness << "\t\t"
  //      << f_distance << "\t\t"
  //      << f_feasibility << "\t\t"
  //      << f_key_view << RESET << endl;
  if (f_mov_objs > 0.1)
    cout << RED_B << "f_mov_objs: " << f_mov_objs << RESET << endl;

  f = lambda_smooth_ * f_smoothness +
      new_lambda_collision_ * f_distance +
      lambda_feasible_ * f_feasibility +
      lambda_mov_obj_ * f_mov_objs +
      lambda_key_view_ * f_key_view;

  Eigen::MatrixXd grad_3D = lambda_smooth_ * g_smoothness +
                            new_lambda_collision_ * g_distance +
                            lambda_feasible_ * g_feasibility +
                            lambda_mov_obj_ * g_mov_objs +
                            lambda_key_view_ * g_key_view;
  memcpy(grad.data(), grad_3D.data() + 3 * order_, n * sizeof(double));
}

void BsplineOptimizer::combineCostRefine(const Eigen::VectorXd &x,
                                         Eigen::VectorXd &grad,
                                         double &f)
{
  size_t n = x.size();
  memcpy(cps_.points.data() + 3 * order_, x.data(), n * sizeof(double));

  /* ---------- evaluate cost and gradient ---------- */
  double f_smoothness, f_fitness, f_feasibility;

  Eigen::MatrixXd g_smoothness = Eigen::MatrixXd::Zero(3, cps_.points.cols());
  Eigen::MatrixXd g_fitness = Eigen::MatrixXd::Zero(3, cps_.points.cols());
  Eigen::MatrixXd g_feasibility = Eigen::MatrixXd::Zero(3, cps_.points.cols());

  // time_satrt = ros::Time::now();

  calcSmoothnessCost(cps_.points, f_smoothness, g_smoothness);
  calcFitnessCost(cps_.points, f_fitness, g_fitness);
  calcFeasibilityCost(cps_.points, f_feasibility, g_feasibility);

  /* ---------- convert to solver format...---------- */
  f = lambda_smooth_ * f_smoothness +
      lambda_fitness_ * f_fitness +
      lambda_feasible_ * f_feasibility;

  Eigen::MatrixXd grad_3D = lambda_smooth_ * g_smoothness +
                            lambda_fitness_ * g_fitness +
                            lambda_feasible_ * g_feasibility;
  memcpy(grad.data(), grad_3D.data() + 3 * order_, n * sizeof(double));
}

/**
 * calculate the cost caused by dynamic obstacles
 */
void BsplineOptimizer::calcMovingObjCost(
    const Eigen::MatrixXd &q, double &cost, Eigen::MatrixXd &gradient)
{
  cost = 0.0;
  double dis_safe = dist_safe_dyn_;

  size_t n_traj = obs_trajs_->size();
  size_t n_obs = dyn_obs_cp_index_.size();
  for (size_t i = 0; i < n_obs; i++)
  {
    bool is_dangerous = false;

    size_t n_pts = dyn_obs_cp_index_[i].size();
    for (size_t j = 0; j < n_pts; j++)
    {
      int index = dyn_obs_cp_index_[i][j];
      Eigen::Vector3d p_delta = q.col(index) - dyn_obs_pos_[i][j];
      double dist = p_delta.norm();
      double delta = dis_safe - dist;

      if (!is_dangerous && dist < dist_attention_)
      {
        is_dangerous = true;
      }

      if (delta > 0)
      {
        cost += pow(delta, 3);
        gradient.col(index) += -3.0 * delta * delta * p_delta.normalized();
      }
    }

    // for dynamic obstacles
    if (i < n_traj) {
      obs_trajs_->at(i)->is_dangerous_ = is_dangerous;
    }
  }
}

/**
 * calculate the cost caused by collision (control points Q stay close to obstacle)
 */
void BsplineOptimizer::calcDistanceCostRebound(const Eigen::MatrixXd &q, double &cost,
                                               Eigen::MatrixXd &gradient, int iter_num,
                                               double smoothness_cost)
{
  cost = 0.0;
  int end_idx = q.cols() - order_;
  double dis_safe = dist_safe_;
  double a = 3 * dis_safe, b = -3 * pow(dis_safe, 2), c = pow(dis_safe, 3);

  force_stop_type_ = DONT_STOP;

  // TODO MZY 为啥搞这个判断条件？
  // TODO 先跳过这个判断条件
  // 0.1 is an experimental value that indicates the trajectory is smooth enough.
  if (iter_num > 3 && smoothness_cost / (cps_.size_ - 2 * order_) < 0.1)
  {
    checkCollisionCoarse();
  }

  /*** calculate distance cost and gradient ***/
  for (int i = order_; i < end_idx; ++i)
  {
    for (size_t j = 0; j < cps_.directions[i].size(); ++j)
    {
      // TODO 能否确保每次计算之前，free_pts/directions 与 points 一一对应?
      // TODO 是不是可能会存在 不同方向的dist相等，相互抵消的情况？
      double dist = (cps_.points.col(i) - cps_.free_pts[i][j]).dot(cps_.directions[i][j]);
      double delta = dis_safe - dist;
      Eigen::Vector3d dist_grad = cps_.directions[i][j];

      if (delta < 0)
      {
        /* do nothing */
      }
      else if (delta < dis_safe)
      {
        cost += pow(delta, 3);
        gradient.col(i) += -3.0 * delta * delta * dist_grad;
      }
      else
      {
        cost += a * delta * delta + b * delta + c;
        gradient.col(i) += -(2.0 * a * delta + b) * dist_grad;
      }
    }
  }
}

void BsplineOptimizer::calcFitnessCost(
    const Eigen::MatrixXd &q, double &cost, Eigen::MatrixXd &gradient)
{
  cost = 0.0;

  int end_idx = q.cols() - order_;

  // def: f = |x*v|^2/a^2 + |x×v|^2/b^2
  double a2 = 25, b2 = 1;
  for (auto i = order_ - 1; i < end_idx + 1; ++i)
  {
    Eigen::Vector3d x =
        (q.col(i - 1) + 4 * q.col(i) + q.col(i + 1)) / 6.0 - ref_pts_[i - 1];
    Eigen::Vector3d v = (ref_pts_[i] - ref_pts_[i - 2]).normalized();

    double xdotv = x.dot(v);
    Eigen::Vector3d xcrossv = x.cross(v);

    double f = pow((xdotv), 2) / a2 + pow(xcrossv.norm(), 2) / b2;
    cost += f;

    Eigen::Matrix3d m;
    m << 0, -v(2), v(1), v(2), 0, -v(0), -v(1), v(0), 0;
    Eigen::Vector3d df_dx = 2 * xdotv / a2 * v + 2 / b2 * m * xcrossv;

    gradient.col(i - 1) += df_dx / 6;
    gradient.col(i) += 4 * df_dx / 6;
    gradient.col(i + 1) += df_dx / 6;
  }
}

/**
 * calculate the cost from smoothness of b-spline by its control points Q
 */
void BsplineOptimizer::calcSmoothnessCost(
    const Eigen::MatrixXd &q, double &cost,
    Eigen::MatrixXd &gradient, bool falg_use_jerk /* = true*/)
{
  cost = 0.0;

  if (falg_use_jerk)
  {
    Eigen::Vector3d jerk, temp_j;

    for (int i = 0; i < q.cols() - 3; i++)
    {
      /* evaluate jerk */
      jerk = q.col(i + 3) - 3 * q.col(i + 2) + 3 * q.col(i + 1) - q.col(i);
      cost += jerk.squaredNorm();
      temp_j = 2.0 * jerk;
      /* jerk gradient */
      gradient.col(i + 0) += -temp_j;
      gradient.col(i + 1) += 3.0 * temp_j;
      gradient.col(i + 2) += -3.0 * temp_j;
      gradient.col(i + 3) += temp_j;
    }
  }
  else
  {
    Eigen::Vector3d acc, temp_acc;

    for (int i = 0; i < q.cols() - 2; i++)
    {
      /* evaluate acc */
      acc = q.col(i + 2) - 2 * q.col(i + 1) + q.col(i);
      cost += acc.squaredNorm();
      temp_acc = 2.0 * acc;
      /* acc gradient */
      gradient.col(i + 0) += temp_acc;
      gradient.col(i + 1) += -2.0 * temp_acc;
      gradient.col(i + 2) += temp_acc;
    }
  }
}

void BsplineOptimizer::calcTerminalCost(
    const Eigen::MatrixXd &q, double &cost, Eigen::MatrixXd &gradient)
{
  cost = 0.0;

  // zero cost and gradient in hard constraints
  Eigen::Vector3d q_3, q_2, q_1, dq;
  q_3 = q.col(q.cols() - 3);
  q_2 = q.col(q.cols() - 2);
  q_1 = q.col(q.cols() - 1);

  dq = 1 / 6.0 * (q_3 + 4 * q_2 + q_1) - local_target_pt_;
  cost += dq.squaredNorm();

  gradient.col(q.cols() - 3) += 2 * dq * (1 / 6.0);
  gradient.col(q.cols() - 2) += 2 * dq * (4 / 6.0);
  gradient.col(q.cols() - 1) += 2 * dq * (1 / 6.0);
}

/**
 * calculate cost from dynamic limits
 *
 * @note not use the formulation in paper EGO-PLANNER (formulation (10))
 */
void BsplineOptimizer::calcFeasibilityCost(const Eigen::MatrixXd &q,
                                           double &cost,
                                           Eigen::MatrixXd &gradient)
{
  cost = 0.0;

  /* abbreviation */
  double ts, ts_inv, ts_inv2, ts_inv3;
  ts = bspline_interval_;
  ts_inv = 1 / ts;
  ts_inv2 = 1 / ts / ts;
  ts_inv3 = 1 / ts / ts / ts;

  /* velocity feasibility */
  for (int i = 0; i < q.cols() - 1; i++)
  {
    Eigen::Vector3d v = (q.col(i + 1) - q.col(i)) * ts_inv;

    for (int j = 0; j < 3; j++)
    {
      if (v(j) > max_vel_)
      {
        // multiply ts_inv3 to make vel and acc has similar magnitude
        cost += pow(v(j) - max_vel_, 2) * ts_inv2;

        gradient(j, i + 0) += -2 * (v(j) - max_vel_) * ts_inv3;
        gradient(j, i + 1) += 2 * (v(j) - max_vel_) * ts_inv3;
      }
      else if (v(j) < -max_vel_)
      {
        cost += pow(v(j) + max_vel_, 2) * ts_inv2;

        gradient(j, i + 0) += -2 * (v(j) + max_vel_) * ts_inv3;
        gradient(j, i + 1) += 2 * (v(j) + max_vel_) * ts_inv3;
      }
    }
  }

  /* acceleration feasibility */
  for (int i = 0; i < q.cols() - 2; i++)
  {
    Eigen::Vector3d ai = (q.col(i + 2) - 2 * q.col(i + 1) + q.col(i)) * ts_inv2;

    for (int j = 0; j < 3; j++)
    {
      if (ai(j) > max_acc_)
      {
        cost += pow(ai(j) - max_acc_, 2);

        gradient(j, i + 0) += 2 * (ai(j) - max_acc_) * ts_inv2;
        gradient(j, i + 1) += -4 * (ai(j) - max_acc_) * ts_inv2;
        gradient(j, i + 2) += 2 * (ai(j) - max_acc_) * ts_inv2;
      }
      else if (ai(j) < -max_acc_)
      {
        cost += pow(ai(j) + max_acc_, 2);

        gradient(j, i + 0) += 2 * (ai(j) + max_acc_) * ts_inv2;
        gradient(j, i + 1) += -4 * (ai(j) + max_acc_) * ts_inv2;
        gradient(j, i + 2) += 2 * (ai(j) + max_acc_) * ts_inv2;
      }
    }
  }
}

/**
 * calculate the cost in the ESDF map
 */
void BsplineOptimizer::calcESDFReboundCost(const Eigen::MatrixXd &q, double &cost, Eigen::MatrixXd &gradient)
{
  cost = 0.0;
  constexpr double dist_threshold = 0.8; // TODO
  double dist;
  Eigen::Vector3d dist_grad;

  for (int i = order_; i < q.cols(); i++)
  {
    if (!grid_map_->evaluateESDFWithGrad(q.col(i), dist, dist_grad))
      continue;

    if (dist < dist_threshold)
    {
      cost += pow(dist - dist_threshold, 2);
      gradient.col(i) += 2.0 * (dist - dist_threshold) * dist_grad;
    }
  }
}

/**
 * Calculate the cost from visibility
 * The formulation from paper (14-15), but use cubic penalty term
 */
void BsplineOptimizer::calcVisibilityReboundCost(const Eigen::MatrixXd &q,
                                                 double &cost,
                                                 Eigen::MatrixXd &gradient)
{
  cost = 0.0;

  double dist;
  Eigen::Vector3d dist_grad;
  Eigen::Vector3d grad_vis;

  vis_pk_grad.clear();
  vis_pk_grad_real.clear();
  vis_pk.clear();

  for (size_t i_attract = 0; i_attract < occlusion_index.size(); i_attract++)
  {
    int i = occlusion_index[i_attract];
    Eigen::Vector3d swarm_pos = occlusion_target_point[i_attract];
    Eigen::Vector3d now_pos = cps_.points.col(i);

    int sample = 10;
    double dist_threshold = 1.0;
    double line_norm = (now_pos - swarm_pos).norm();

    for (int kk = 1; kk <= sample; kk++)
    {
      double lambda_k = kk * 1.0 / sample;
      Eigen::Vector3d pk = lambda_k * now_pos + (1 - lambda_k) * swarm_pos;
      double threshold_k = dist_threshold * (1 - lambda_k) * line_norm;

      grid_map_->evaluateESDFWithGrad(pk, dist, dist_grad);
      if (dist_grad.norm() > 1e-4)
        dist_grad.normalize();

      if (dist < threshold_k)
      {
        cost += pow(threshold_k - dist, 3);
        /* Derivative the above formula */
        grad_vis = -3 * (threshold_k - dist) * (threshold_k - dist) *
                   (lambda_k * dist_grad - dist_threshold * (1 - lambda_k) / line_norm * (now_pos - swarm_pos));

        gradient.col(i) += grad_vis;

        vis_pk.push_back(pk);
        vis_pk_grad.push_back(dist_grad);
        vis_pk_grad_real.push_back(-dist_threshold * (1 - lambda_k) / line_norm * (now_pos - swarm_pos));
      }
    }

    occlusion_gradient[i_attract] = gradient.col(i);
  }
}

void BsplineOptimizer::calcKeyViewPtCost(
    const Eigen::MatrixXd &q, double &cost, Eigen::MatrixXd &gradient)
{
  cost = 0.0;

  if (ros::Time::now() + ros::Duration(0.3) > t_key_view_) return;

  int start_id = order_;
  int end_id = q.cols() - order_;

  // fine closest point to key_view_pt_
  int id_min = -1;
  double dis_min = std::numeric_limits<double>::max();
  for (int i = start_id; i < end_id; i++) {
    double dis = (q.col(i) - key_view_pt_).norm();
    if (dis > 1.0) continue;
    if (dis < dis_min) {
      dis_min = dis;
      id_min = i;
    }
  }

  if (id_min == -1) return;

  // limit the velocity near the key view point
  /**
   * Vel near Q(i) is V(i-1) and V(i)
   *  V(i-1) = (Q(i) - Q(i-1)) / ts
   *  V(i) = (Q(i+1) - Q(i)) / ts
   */
  double ts, ts_inv, ts_inv2, ts_inv3;
  ts = bspline_interval_;
  ts_inv = 1 / ts;
  ts_inv2 = ts_inv * ts_inv;
  ts_inv3 = ts_inv * ts_inv2;

  Eigen::Vector3d v = 0.5 * (q.col(id_min + 1) - q.col(id_min - 1)) * ts_inv;
  for (int j = 0; j < 3; j++) {
    // multiply ts_inv3 to make vel and acc has similar magnitude
    if (v(j) > max_vel_key_view_) {
      cost += pow(v(j) - max_vel_key_view_, 2) * ts_inv2;
      double val = (v(j) - max_vel_key_view_) * ts_inv3;
      gradient(j, id_min - 1) += -val;
      gradient(j, id_min + 1) += val;
    }
    else if (v(j) < -max_vel_key_view_) {
      cost += pow(v(j) + max_vel_key_view_, 2) * ts_inv2;
      double val = (v(j) + max_vel_key_view_) * ts_inv3;
      gradient(j, id_min - 1) += -val;
      gradient(j, id_min + 1) += val;
    }
  }
}

// TODO TEST
void BsplineOptimizer::saveAstarToTxt(const std::vector<Eigen::Vector3d> &vec,
                                      const std::string &filename)
{
  std::ofstream file(filename, std::ios::app);

  if (!file.is_open())
  {
    std::cerr << "Error: Unable to open file " << filename << std::endl;
    return;
  }
  file.setf(std::ios::fixed, std::ios::floatfield);
  file << "-------------------------------" << std::endl;
  file << "time: " << ros::Time::now().toSec() << std::endl;
  file << "astar" << std::endl;
  for (const auto &v : vec)
  {
    file << v.x() << " " << v.y() << std::endl;
  }

  file.close();
}

// TODO TEST
void BsplineOptimizer::saveConotrolPointsToTxt(const Eigen::MatrixXd &matrix,
                                               const std::string &filename)
{
  if (matrix.rows() < 2)
  {
    std::cerr << "Error: matrix.rows() < 2" << std::endl;
    return;
  }
  std::ofstream file(filename, std::ios::app);

  if (!file.is_open())
  {
    std::cerr << "Error: Unable to open file " << filename << std::endl;
    return;
  }

  file << "conotrolpoints" << std::endl;
  for (int i = 0; i < matrix.cols(); ++i)
  {
    for (int j = 0; j < 2; ++j)
    {
      file << matrix(j, i); // 注意，这里是先行后列
      if (j != 2 - 1)
      {
        file << " ";
      }
    }
    file << std::endl;
  }

  file.close();
}

} // namespace ego_planner
